
import lejos.nxt.TouchSensor;
import lejos.robotics.subsumption.*;
import lejos.robotics.navigation.*;


public class HitWall implements Behavior {
	
	DifferentialPilot navigator;
	TouchSensor bumperLeft, bumperRight;
	
	public HitWall(DifferentialPilot nav, TouchSensor bl, TouchSensor br) {
		navigator = nav;
		bumperLeft = bl;
		bumperRight = br;
	}
	
	public boolean takeControl() {
		return bumperLeft.isPressed() || bumperRight.isPressed();
	}
	
	public void suppress() {
		
	}
	
	public void action() {
		navigator.setAcceleration(10000);
		navigator.travel(-25);
		
		if (bumperLeft.isPressed()) {
			navigator.steer(20, 75, true);
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			navigator.steer(-20, -75, true);
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		} else {
			navigator.steer(-20, -75, true);
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			navigator.steer(20, 75, true);
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		navigator.setAcceleration(1000);
	}
}
